#include <opencv2/opencv.hpp>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <chrono>

using namespace std;
using namespace cv;
using namespace chrono;

Mat canvas = Mat::zeros(Size(950, 900), CV_8UC1);
int step_max = 10;//斐波那契球点个数
int self_r_max = 60; //自转分辨率，将360度分为self_r_max份
Point2d offset{470, 470};   //画图用
double scalar = 7;         //画图用，控制模型比例

void read_stl(string path, vector<double>& triangles)
{
    ifstream stl(path, fstream::binary);
    stl.seekg(80);
    char _head[4];
    stl.read(_head, 4);
    stl.seekg(84);
    uint32_t triangle_size;
    memmove(&triangle_size, _head, 4);
    vector<double> points(triangle_size * 50);
    stl.read(&points[0], triangle_size * 50);
    triangles.resize(9 * triangle_size);
    float point3f_3[9];

    for (int i = 0; i < triangle_size; i++)
    {
        memmove(point3f_3, &(points[i * 50 + 12]), 36);
        for (size_t j = 0; j < 3; j++)
            for (size_t k = 0; k < 3; k++)
                triangles[i * 9 + j * 3 + k] = point3f_3[j * 3 + k];
    }
}

void draw_stl_xy(Mat &canvas, vector<double> &triangles, int is_fill_triangle=0)    //最后一个参数控制是否涂满每个三角面
{
    Point2d p[3];

    for (int i = 0; i < triangles.size()/9; i++)
    {
        vector<Point> triangle;
        for (size_t j = 0; j < 3; j++)
        {
            p[j] = {triangles[i * 9 + j * 3 + 0], triangles[i * 9 + j * 3 + 1]};
            p[j] *= scalar;
            p[j] += offset;
            if (is_fill_triangle)triangle.emplace_back(p[j]);
        }
        if (is_fill_triangle)
            drawContours(canvas, vector<vector<Point>>{triangle}, -1, Scalar(255), FILLED);
        else
        {
            line(canvas, p[0], p[1], Scalar(255));
            line(canvas, p[1], p[2], Scalar(255));
            line(canvas, p[0], p[2], Scalar(255));
        }
    }
}

void rotate_stl(vector<double> &triangles_in, vector<double> &triangles_out, vector<double> &r_vector)
{
    Mat r_matrix; //轴角矩阵
    Rodrigues(r_vector, r_matrix);
    Mat in_matrix = Mat(triangles_in).reshape(1, triangles_in.size()/3);
    Mat out_matrix = in_matrix*(r_matrix.t());
    triangles_out = out_matrix.reshape(1, 1);
}

void fibonacci_sphere(int index,int max,vector<double>& point)
{
    double phi = 3.1415926 * (3 - sqrt(5));
    double radius, theta;

    if(point.size()!=3)return;
    point[1] = 1 - (index / 1.0 / max) * 2;
    radius = sqrt(1 - point[1] * point[1]);
    theta = phi * index;
    point[0] = cos(theta) * radius;
    point[2] = sin(theta) * radius;
}

int main(int argc, char const *argv[])
{
    vector<double> triangles_all;
    vector<double> triangles_rt;
    vector<double> triangles_rt_self;

    read_stl("1.stl", triangles_all);

    namedWindow("a",WINDOW_NORMAL);
    vector<double> r_vector(3); //轴角的轴
    vector<double> self_r_vector(3); //自转轴角的轴
    vector<double> product_v(3); //yaw{0,1,0}与均点向量的叉乘，公式经过化简，经过正则化到1
    Mat _canvas;        //空画布
    vector<double> yaw{0,0,1};
    for(int k=0;k<step_max;k++)
    {
        auto start = chrono::steady_clock::now();

        fibonacci_sphere(k, step_max, r_vector);

        product_v = {
            yaw[1] * r_vector[2] - yaw[2] * r_vector[1],
            yaw[2] * r_vector[0] - yaw[0] * r_vector[2],
            yaw[0] * r_vector[1] - yaw[1] * r_vector[0]};

        normalize(product_v, product_v, acos(yaw[0]*r_vector[0]+yaw[1]*r_vector[1]+yaw[2]*r_vector[2]), NORM_L2); //角度公式已经简化

        rotate_stl(triangles_all, triangles_rt, product_v);
        triangles_rt_self = triangles_rt;

        for (size_t n = 0; n < self_r_max; n++)
        {
            double self_scalar = 3.1415926 * 2 * n / self_r_max;
            self_r_vector[0] = r_vector[0] * self_scalar;
            self_r_vector[1] = r_vector[1] * self_scalar;
            self_r_vector[2] = r_vector[2] * self_scalar;
            rotate_stl(triangles_rt, triangles_rt_self, self_r_vector);
            
            _canvas = canvas.clone();
            //最后一个参数选择是否填涂三角面，由于纯代码实现，没有用图形库，所以会比较慢
            draw_stl_xy(_canvas, triangles_rt_self /*,true*/);
            //画一下自转轴
            line(_canvas, offset + 1500 * Point2d{self_r_vector[0], self_r_vector[1]}, offset + Point2d{0, 0}, Scalar(255), 2);
            imshow("a", _canvas);
            waitKey(1);
        }
        auto end = chrono::steady_clock::now();
        double diff_sec = chrono::duration_cast<chrono::duration<double>>(end - start).count();
        cout << diff_sec * 1000 << endl;
    }

    waitKey(0);

    return 0;
}
